/**
 * The Woosta Table Football Engine
 *
 * License... etc.
 **
 * Physics Engine
 *
 * Provides the following:
 *	- Base classes for 'Physical' and 'Static' objects,
 *	  which represent movable and non-movable objects respectively;
 *	- Collision detection between physical and static objects;
 *	- Linear and angular velocity, which update an object's position
 *	  and orientation automatically;
 *	- Forces and impulses, which affect the linear and angular velocity
 *	  based upon force direction and position relative to the object's
 *	  centre of mass.
 **
 * Author: Sean Micklethwaite
 **/

#ifndef __physicsH__
#define __physicsH__

#include "config.h"


#ifdef __PHYSICS_INTERNAL__

#include "dynamic.h"
#include "texture.h"
#include "3d.h"
#include "prioheap.h"
#include <set>
#include <vector>
#include <map>

/// Maximum number of collisions before treated as contact.
#define INTERACTION_THRESHOLD	8

namespace WtfEngine 
{
	class PhysicalModel : public FBOTexture
	{
	public:
		typedef GcReference<PhysicalModel> tRef;

	protected:
		tScalar		mBoundingRadius;
		int			mNumVertices;
		tScalar		mMassQuot;
		
		/// Highest and lowest points in each dimension, respectively.
		/// Used to calculate air & fluid resistances.
		tVector		mResistancePoints[2];
		
		Orientation3D	mLocalInverseInertiaTensor;
		
		/// Local vertices are the 'zero' positions of each vertex, and are stored local
		/// to this model. The mzVertex and mzNextVertex are pointers to vertex arrays local to
		/// a particular object, where NextVertex are the vertices after the the current time step.
		tVector*	mzLocalVertex, *mzVertex, *mzNextVertex;
		
		void Create(int numVertices, tScalar density = 1.0);

	public:
		PhysicalModel(tScalar r)
			: mBoundingRadius(r), mzVertex(NULL), mNumVertices(0) {};
		~PhysicalModel();

		bool CheckBounds(const PhysicalModel& obj,
						 const tVector& disp,
						 const tVector& vel) const;

		bool CheckCollision(const WtfGraphics::Mesh& obj,
			const tVector& disp, // Displacement of obj, to our origin
			const tVector& meshVel, // Mesh velocity
			const tOrientation& meshOri, // Mesh orientation
			tVector& point, tVector& normal, tVector& relVel, tScalar& time) const;

		void UpdateModel(const Orientation3D &ori,
			const tVector& vel, const Orientation3D &angVel, tScalar dt, tVector *zvVertex, tVector *zvNextVertex) const;

		tScalar getMassQuot() const{
			return mMassQuot;
		};

		const Orientation3D& getLocalInverseInertiaTensor() const{
			return mLocalInverseInertiaTensor;
		};
		
		const tVector* AllocateVertexArrays(tVector*& rpV, tVector*& rpU){
			rpV = new tVector[mNumVertices];
			rpU = new tVector[mNumVertices];
			return NULL;
		};
		
		inline const tVector* getVertexArray() const{
			return mzLocalVertex;
		};
		inline void setVertexArrays(tVector *pV, tVector *pU){
			mzVertex = pV; mzNextVertex = pU;
		};
		
		const tVector* getResistancePoints(){
			return mResistancePoints;
		};
	};

	/**** Forward Declarations ****/

	class Static;
	class Physical;
	class PhysicalGroup;
	class Force;
	class CollisionHeap;

	class Collision : public Event
	{
	public:
		typedef GcReference<Collision>	tRef;

	protected:
		tVector			relativeVelocity;
		tVector			normal, point;
		Physical*		physObject;
		Static*			staticObject;
		Physical*		movingObject;
		tScalar			time;

	public:
		Collision(Physical& mobj, Physical& pobj, tVector& rv, tVector& p, tVector& norm,
				tScalar t)
			: movingObject(&mobj), physObject(&pobj), staticObject(NULL),
			relativeVelocity(rv), normal(norm), point(p), time(t) {};

		Collision(Physical& mobj, Static& sobj, tVector& rv, tVector& p, tVector& norm,
				tScalar t)
			: movingObject(&mobj), physObject(NULL), staticObject(&sobj),
			relativeVelocity(rv), normal(norm), point(p), time(t) {};

		void processCollision(CollisionHeap& heap);
		bool relatesTo(Physical *pObject) const{
			return (pObject == movingObject || pObject == physObject);
		};
	};

	class CollisionHeap : public Rtl::PriorityQueue<Collision, tScalar>
	{
	public:
		// Remove all collisions relating to the passed object
		void RemoveObject(Physical *pObject);
	};

	
	class ObjectConnectionEvent : public ObjectInteractionEvent
	{
		tPosition	vPosition;
		
	public:
		ObjectConnectionEvent(GameObject *pConnector, GameObject *pConnectee, const tPosition& pos)
			: ObjectInteractionEvent(pConnector, pConnectee), vPosition(pos){
		};
		
		const tPosition& getPosition(){
			return vPosition;
		};
	};
	
	class ObjectDisconnectionEvent : public ObjectInteractionEvent
	{
	public:
		ObjectDisconnectionEvent(GameObject *pConnector, GameObject *pConnectee)
			: ObjectInteractionEvent(pConnector, pConnectee){
		};
	};
	
	
	/**
	 * A class representing a group of connected physical & static objects,
	 * each connected in a pairwise fashion via a 'Connection'.
	 * The system is resolved before object update, in order to balance
	 * the internal forces based upon the external forces acting upon the
	 * system.
	 *
	 * This class provides the basis for all rigid body dynamics.
	 **/
	class PhysicalSystem : public GcObject
	{
	private:
		struct Connection
		{
			Physical *		pConnector;
			
			/// Connection point, relative to the connector
			tPosition		vPosition;
			
			/// Direction of connection, facing the connector
			tDirection		vNormal;
			
			/// Maximum magnitude of the force on the connection, in the direction of the
			/// connection. Forces opposite to the direction of the connection (i.e. into
			/// the joint) are assumed to have infinite strength.
			tScalar			fStrength;
			
			/// Amount of damping for motion perpendicular to the direction of the connection.
			tScalar			fResistanceQuot;
			
			/// Used for evaluation
			int				iConstraint;
			int				iStage;
			
			/// Force on the connector at this connection point
			tVector			vForce;

			Connection(Physical* pCr, const tVector& position, const tDirection& normal,
				tScalar	strength, tScalar resistance)
				: pConnector(pCr), vPosition(position), vNormal(normal), fStrength(strength),
				fResistanceQuot(resistance) {};
		};
		
		struct PhysicalConnection : public Connection
		{
			Physical *pConnectee;

			PhysicalConnection(Physical* pCr, Physical* pCe,
				const tVector& position, const tDirection& normal,
				tScalar	strength, tScalar resistance)
				: pConnectee(pCe),
				Connection(pCr, position, normal, strength, resistance) {};
			
			void Apply();
		};
		
		struct StaticConnection : public Connection
		{
			Static *  pConnectee;

			StaticConnection(Physical* pCr, Static* pCe,
				const tVector& position, const tDirection& normal,
				tScalar	strength, tScalar resistance)
				: pConnectee(pCe),
				Connection(pCr, position, normal, strength, resistance) {};
			
			void Apply();
		};
		
		typedef std::map<Physical*, PhysicalConnection*>
				tPhysicalConnectionList;
		typedef std::map<Physical*, StaticConnection*>
				tStaticConnectionList;
		
		
		tPhysicalConnectionList		mPhysicalConnections;
		tStaticConnectionList		mStaticConnections;
		
		tScalar					mSystemTime;
		
		static std::set<PhysicalSystem*>	gSystems;

	public:
		PhysicalSystem();
		~PhysicalSystem();
		
		void		ResolveSystem();
		
		void EvalPhysical(PhysicalConnection *pCon, tVector vForce, int stage, int n);
		void EvalStatic(StaticConnection *pCon, tVector vForce, int stage, int n);
		
		void 		InsertConnection(PhysicalConnection* pConnection);
		void 		InsertConnection(StaticConnection* pConnection);
		void 		RemoveConnection(Physical* pConnector, Physical* pConnectee);
		void 		RemoveConnection(Physical* pConnector, Static* pConnectee);
		
		bool		hasConnection(Physical *pConnector, Static * pConnectee, const tPosition& vPos);
		bool		hasConnection(Physical *pConnector, Physical * pConnectee, const tPosition& vPos);
		
		static void Update();
		
		static bool Connect(Physical* pConnector, Physical* pConnectee,
			const tVector& pos, const tVector& norm, const tVector& relVel,
			tScalar strength, tScalar resistance);
		static bool Connect(Physical* pConnector, Static* pConnectee,
			const tVector& pos, const tVector& norm, const tVector& relVel,
			tScalar strength, tScalar resistance);

		typedef GcReference<PhysicalSystem>	tRef;
	};
	

	class Physical : public Renderable3D
	{
	public:
		typedef GcReference<Physical>	tRef;

		struct Force : public GcObject, public tVector
		{
			typedef GcReference<Force>	tRef;

			tVector		relativePosition;
			tGameTime	remainingTime;

			Force()
				: tVector(), relativePosition() {};
			Force(const tVector& f)
				: tVector(f), relativePosition() {};
			Force(const tVector& f, const tVector& p)
				: tVector(f), relativePosition(p) {};
		};

	protected:
		PhysicalModel::tRef				mModel;
		
		tPosition 			currentPosition;
		Orientation3D		currentOrientation;
		
		tVector				currentForce;
		tVector				currentVelocity;
		tVector				mLastVelocity;
		tVector				mAngularMomentum;
		tVector				mTorque;

		tVector*			mzVertex;
		tVector*			mzNextVertex;
		
		Orientation3D		mInverseInertiaTensor;
		Orientation3D		mAngularVelocity;

		tGameTime			mLocalTime;

		std::set<Force::tRef>	attachedForces;

		bool				isActive;
		bool				isParticle;

		PhysicalSystem::tRef	mrPhysicalSystem;
		
		unsigned				mNumInteractions;
		
		friend class PhysicsEngine;
		friend class PhysicalSystem;
		friend class Static;
		
	public:
		Physical(const WtfGraphics::GraphicsData::tRef d,
			const PhysicalModel::tRef m, bool isParticle = false);
		~Physical() {};

		void UpdateForces();
		void IntegrateForces(tScalar dt);
		void UpdatePhysical(tScalar dt, bool endFrame = false);

		void applyForce(tVector force, tVector pos);
		void applyForce(Force::tRef	force) {
			attachedForces.insert(force);
		};
		void applyImpulse(tVector impulse, tVector point);

		bool CheckCollision(Physical &obj,
							tVector& point,
							tVector& normal,
							tVector& relVel,
							tScalar& time);
		
		void DoCollisionDetection(CollisionHeap& heap);

		const tVector& getVelocity() const {
			return currentVelocity;
		};
		const tVector& getResultantForce() const {
			return currentForce;
		};
		tVector getAcceleration() const {
			tVector vtmp = currentForce * getMassQuot();
			return vtmp;
		};
		const Orientation3D& getInverseInertiaTensor() const{
			return mInverseInertiaTensor;
		};
		tScalar	getMassQuot() const{
			return mModel->getMassQuot();
		};

		bool hasPhysicalSystem() const{
			return !mrPhysicalSystem.isNull();
		};
		void setPhysicalSystem(PhysicalSystem* pSys){
			mrPhysicalSystem = pSys;
		};
		PhysicalSystem*	getPhysicalSystem() const{
			return &*mrPhysicalSystem;
		};

		void setActive();
		
		void setVelocity(const tVector& v);
		void setAngularMomentum(const tVector& mom);

		const PhysicalModel& getModel() const {
			return *mModel;
		};
		
		void setPosition(tVector pos) { currentPosition = pos; }
		void setOrientation(Orientation3D ori) { currentOrientation = ori; }

		tVector getPosition() { return currentPosition; }
		Orientation3D getOrientation() { return currentOrientation; }
		
		virtual void Destroy();
	};

	/**
	 * Represents a non-movable object
	 **/
	class Static : public Renderable3D
	{
		tPosition 		currentPosition;
		Orientation3D	currentOrientation;
		
	public:
		typedef GcReference<Static>	tRef;

	public:
		Static(const WtfGraphics::GraphicsData::tRef& d);
		~Static() {};

		bool CheckCollision(Physical &obj,
							tVector& point,
							tVector& normal,
							tVector& relVel,
							tScalar& time);
		
		void setPosition(tVector pos) { currentPosition = pos; }
		void setOrientation(Orientation3D ori) { currentOrientation = ori; }

		tVector getPosition() { return currentPosition; }
		Orientation3D getOrientation() { return currentOrientation; }
		
		virtual void Destroy();
	};

	/**
	 * Fired when two objects collide.
	 **/
	class ObjectCollisionEvent : public ObjectInteractionEvent
	{
	public:
		ObjectCollisionEvent(GameObject * pObj, GameObject * pIntObj)
			: ObjectInteractionEvent(pObj, pIntObj) {};
		
		virtual bool Accepts (const Event& ev){
			const ObjectCollisionEvent*	pEv = dynamic_cast<const ObjectCollisionEvent*>(&ev);
			return ((pEv != NULL) && this->ObjectInteractionEvent::Accepts(ev));
		};
	};
	
	class CollisionDetection : public Task<CollisionDetection>
	{
	public:
		CollisionDetection()
			: Task<CollisionDetection>(500, "CollisionDetection"){};


	protected:
		void Run();
	};

	class PhysicsEngine : public Task<PhysicsEngine>
	{
	public:	
		typedef ObjectList<Physical::tRef>	tActiveList;
		typedef ObjectList<Physical::tRef>	tPassiveList;
		typedef ObjectList<Static::tRef>	tStaticList;

	private:
		tActiveList				mActiveList;
		tPassiveList			mPassiveList;
		tStaticList				mStaticList;

	public:
		PhysicsEngine()
			: Task<PhysicsEngine>(501, "PhysicsEngine") {};

		inline void PushPhysical(Physical::tRef pObj) {
			mPassiveList.PushObject(pObj);
		};

		inline void RemovePhysical(Physical::tRef rObj) {
			if(rObj->isActive) mActiveList.remove(rObj);
			mPassiveList.remove(rObj);
		};
		
		inline void PushStatic(Static::tRef pObj) {
			mStaticList.PushObject(pObj);
		};

		inline void RemoveStatic(Static::tRef rObj) {
			mStaticList.remove(rObj);
		};
		
		inline void SetActive(const Physical::tRef& pObj) {
#ifndef NCHECKS
			if(pObj->isActive)
				throw std::logic_error("Object is already active!");
#endif
			pObj->isActive = true;
			mActiveList.PushObject(pObj);
		};

		inline void SetPassive(const Physical::tRef& pObj) {
			pObj->isActive = false;
			mActiveList.remove(pObj);
		};

		inline tActiveList& GetActive() {
			return mActiveList;
		};

		inline tPassiveList& GetPassive() {
			return mPassiveList;
		};

		inline tStaticList& GetStatic() {
			return mStaticList;
		};

	protected:
		void Run();
	};
};

#elif defined(__PHYSICS_BULLET__)
#	include "physics_ext_bullet.h"
#else
#	include "physics_none.h"
#endif

#endif
